#!/usr/bin/env python

'''

这个节点用来
接收obj_pose(base_link下原始数据，需分情况做对应计算，如抓取、末端跟踪验算)
接收machine_put_pose、 handover_pose，规划执行放置轨迹，并在到位后判断是否到位
接收任务指令，用来启动
接收消息保存到全局变量，状态转移用单独的线程，保证数据不阻塞，

状态转移/任务管理：
四个大类任务：grasp、put_on_table、 handover 、 wait_for_command
两大类状态：grasp 、 release
细分状态：
stand_by
grasp_arm_go
grasp_arm_arrive  状态转换前判断是否足够接近目标
grasp_hand_arrive
grasp_finger_arrive
grasp_stand_by      task转换

put_arm_go
put_arm_arrvie  状态转换前判断是否足够接近目标
put_hand_arrive
put_finger_arrive
finished        需要根据coffee_state
stand_by

'''

###############     12.24 
###############     stand_by 位姿需要选一个
###############     y轴朝向咖啡机 法向量计算
###############     递给人手 需要知道方向 pre_handover_pose
###############     计算误差的测试 和调参 可用tf可视化

###############     12.26  v2 修改
###############     由于手放置到咖啡机后会卡住，伸展不开，需要由左手来按按钮
###############     所以 右手变成 一次抓取 一次放置

###############     12.28 更新一下问题
###############     stand_by 位姿找一个先
###############     把import和全局变量检查加进来
###############     调整容忍判断、 咖啡机放置的可行偏移，验证倾斜调整的正确性，规划失败后怎么重复
###############     左手按咖啡机的程序
#################   服务器：
##############      把咖啡机的程序串进来
##############      把咖啡机杯子预期发client
import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Bool
from moveit_commander import MoveGroupCommander, Constraints
from geometry_msgs.msg import PoseStamped, TransformStamped
import numpy as np
from scipy.spatial.transform import Rotation
from tf2_ros import TransformListener, Buffer
from moveit_commander.robot_trajectory import RobotTrajectory
from tf_transformations import quaternion_multiply, quaternion_inverse
from moveit_commander.robot_trajectory import RobotTrajectory
from moveit_commander.robot_trajectory import RobotTrajectory
from moveit_commander.robot_trajectory import RobotTrajectory
from tf_transformations import quaternion_matrix
import copy

############################# 全局变量 ###########################
obj_pose_msg = None
button_pose_msg = None
machine_cup_pose_msg = None
handover_pose_msg = None

tool_quat_compensate = None


class CoffeeTaskManager(Node):

    class CoffeeTaskManager:
        """
        CoffeeTaskManager类用于管理咖啡机的状态和任务。
        它通过初始化设置初始状态，并准备执行各种与咖啡相关的任务。
        """

        def __init__(self):
            """
            初始化CoffeeTaskManager类。
            调用父类的构造方法初始化，并设置咖啡机的初始状态。
            """
            super().__init__('coffee_task_manager')
            self.task_flag = 'wait_for_command'  # 初始化任务标志为等待命令
            self.state_flag = 'stand_by'  # 初始化状态标志为待机
            self.coffee_state = 0  # 初始化咖啡状态为0，表示咖啡机内为空

    def state_refresh(self, new_state):
        """
        刷新状态机的状态。

        根据新的状态更新任务标志（task_flag）和咖啡状态（coffee_state）。
        这个方法主要用于根据机器人当前的状态来决定下一步的任务。

        参数:
        new_state (str): 新的状态，如'grasp_stand_by'、'put_finger_arrive'、'stand_by'等。
        """
        if new_state == 'grasp_stand_by':
            # 当新状态为'grasp_stand_by'时，根据咖啡状态决定任务标志
            if self.coffee_state == 0:
                self.task_flag = "put_on_table"
            else:
                self.task_flag = "handover"
        elif new_state == 'put_finger_arrive':
            # 当新状态为'put_finger_arrive'时，更新咖啡状态或任务标志
            if self.coffee_state == 0:
                self.coffee_state = 1           #################### 暂时先没加左手按咖啡机
            elif self.coffee_state == 1:
                self.task_flag = 'finished'     ############# 放置且是满杯 则转为完成状态
        elif new_state == 'stand_by':
            # 当新状态为'stand_by'时，设置任务标志为等待命令
            self.task_flag = 'wait_for_command'

        # 记录状态标志的更新
        self.get_logger().info(f"Coffee_Task_Manager`s state_flag was {self.state_flag} ")
        self.state_flag = new_state
        self.get_logger().info(f"Coffee_Task_Manager`s state_flag is {self.state_flag} now !!!")
    



# ROS 回调函数
# obj_pose 一直会保证有
def obj_pose_callback(self, msg):
    global obj_pose_msg
    obj_pose_msg = msg

    P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
    quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y,
                         obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]
    # 将四元数转换为旋转矩阵
    R_object = quaternion_matrix(quaternion_object)[:3, :3]
    # 发布obj的tf 可视化
    self.tf_pub("base_link", "obj_link", P_object, R_object)


def tf_pub(self, parent_frame, child_frame, translation, rotation_matrix):
    # 发布 tf 变换 (此函数需要根据ROS2 tf2 实现)
    pass

# button_pose如果掉了 需要异常处理 
# 保持上一有效数据
def button_pose_callback(self, msg):
    global button_pose_msg
    if msg.pose.orientation.x != -1 and msg.pose.orientation.y != -1:
        button_pose_msg = msg
    P_machine = np.array(
        [button_pose_msg.pose.position.x, button_pose_msg.pose.position.y, button_pose_msg.pose.position.z])
    quaternion_machine = [button_pose_msg.pose.orientation.x, button_pose_msg.pose.orientation.y,
                          button_pose_msg.pose.orientation.z, button_pose_msg.pose.orientation.w]
    # 将四元数转换为旋转矩阵
    R_machine = quaternion_matrix(quaternion_machine)[:3, :3]
    # 发布obj的tf 可视化
    self.tf_pub("base_link", "button_link", P_machine, R_machine)

def machine_cup_pose_callback(self, msg):
    global machine_cup_pose_msg
    if msg.pose.orientation.x != -1 and msg.pose.orientation.y != -1:
        machine_cup_pose_msg = msg
    P_machine = np.array([machine_cup_pose_msg.pose.position.x, machine_cup_pose_msg.pose.position.y, machine_cup_pose_msg.pose.position.z])
    quaternion_machine = [machine_cup_pose_msg.pose.orientation.x, machine_cup_pose_msg.pose.orientation.y, machine_cup_pose_msg.pose.orientation.z, machine_cup_pose_msg.pose.orientation.w]
    # 将四元数转换为旋转矩阵
    R_machine = quaternion_matrix(quaternion_machine)[:3, :3]
    # 发布obj的tf 可视化
    self.tf_pub("base_link", "machine_cup_link", P_machine, R_machine)

# handover_pose如果掉了 需要异常处理 
# 保持上一有效数据
def handover_pose_callback(msg):
    global handover_pose_msg
    if msg.pose.orientation.x != -1 and msg.pose.orientation.y != -1:
        handover_pose_msg = msg
    P_handover = np.array([handover_pose_msg.pose.position.x, handover_pose_msg.pose.position.y, handover_pose_msg.pose.position.z])
    quaternion_handover = [handover_pose_msg.pose.orientation.x, handover_pose_msg.pose.orientation.y, handover_pose_msg.pose.orientation.z, handover_pose_msg.pose.orientation.w]
    # 将四元数转换为旋转矩阵
    R_handover = quaternion_matrix(quaternion_handover)[:3, :3]
    # 发布obj的tf 可视化
    tf_pub("base_link", "handover_link", P_handover, R_handover)



def upper_body_callback(msg):
    global coffee_task_manager
    if coffee_task_manager.task_flag == "wait_for_command":
        # 归位
        move_to_stand_by_pose()
        # 更新任务状态 开始任务
        coffee_task_manager.task_flag = msg.data
        task_run()
    else:
        rospy.loginfo(f"Failed to set task flag as task_flag is {coffee_task_manager.task_flag} now !!!")

def left_arm_move_to_pos(position,link_name ):
    global move_group_left_arm

    current_pose = move_group_left_arm.get_current_pose().pose

    move_pose_msg = Pose()
    move_pose_msg.position.x = position[0]
    move_pose_msg.position.y = position[1]
    move_pose_msg.position.z = position[2]
    move_pose_msg.orientation.x = -0.10711095938856535  #button_pose.pose.orientation.x
    move_pose_msg.orientation.y = -0.33227448354515043  #button_pose.pose.orientation.y
    move_pose_msg.orientation.z = -0.845326815249489    #button_pose.pose.orientation.z
    move_pose_msg.orientation.w = 0.40440510059075774   #button_pose.pose.orientation.w

    print("adjusted_right_pose",move_pose_msg)
    print(current_pose)
    
    # 设定一个阈值，如果目标位姿与当前位置的距离大于这个值，则进行分步规划
    step_size = 0.01  # 每一步的距离，单位米
    
    num_steps = 4  # 计算需要的步数

    # 按照分步的策略逐步移动
    for i in range(num_steps):
        # 计算每一步的目标位置
        interpolation_factor = (i + 1) / num_steps  # 计算每一步的插值因子
        intermediate_pose = Pose()
        intermediate_pose.position.x = current_pose.position.x + (position[0] - current_pose.position.x) * interpolation_factor
        intermediate_pose.position.y = current_pose.position.y + (position[1] - current_pose.position.y) * interpolation_factor
        intermediate_pose.position.z = current_pose.position.z + (position[2] - current_pose.position.z) * interpolation_factor

        # 使用当前的位姿插值更新目标姿态
        intermediate_pose.orientation.x = -0.10711095938856535  #button_pose.pose.orientation.x
        intermediate_pose.orientation.y = -0.33227448354515043  #button_pose.pose.orientation.y
        intermediate_pose.orientation.z = -0.845326815249489    #button_pose.pose.orientation.z
        intermediate_pose.orientation.w = 0.40440510059075774   #button_pose.pose.orientation.w

        move_group_left_arm.set_pose_target(intermediate_pose)
        plan, traj, planning_time, error_code = move_group_left_arm.plan()

        if plan:
            success = move_group_left_arm.execute(traj, wait=True)
            move_group_left_arm.stop()
            move_group_left_arm.clear_pose_targets()
            #rospy.sleep(0.2)
            if not success:
                rospy.logwarn(f"Failed to move {link_name} to intermediate pose at step {i+1}.")
                return  # 如果某一步执行失败，则终止
        else:
            move_group_left_arm.stop()
            move_group_left_arm.clear_pose_targets()
            #rospy.sleep(0.2)
            rospy.logwarn(f"Failed to generate plan for {link_name} at step {i+1}.")
            return  # 如果某一步规划失败，则终止

    rospy.loginfo(f"Successfully moved {link_name} to the target pose in {num_steps} steps.")

    

def press_button_callback():

    global move_group_left_arm
    ## 开始左臂按键任务
    button_pose = copy.deepcopy(button_pose_msg)

    button_pose_quaternion = [button_pose.pose.orientation.x, button_pose.pose.orientation.y, button_pose.pose.orientation.z, button_pose.pose.orientation.w]
    button_pose_rotation_matrix = quaternion_matrix(button_pose_quaternion)
    
    local_offset_far = np.array([-0.01, 0.01, 0.10, 1.0])  # [世界坐标系y轴（），世界坐标系z轴（数据增加z轴值减小，向物体下方移动），世界坐标系x轴（数据增加x轴值减少，离物体越远）]
    local_offset_near = np.array([-0.01, 0.01, 0.00, 1.0])  # [世界坐标系y轴（），世界坐标系z轴（数据增加z轴值减小，向物体下方移动），世界坐标系x轴（数据增加x轴值减少，离物体越远）]
    world_offset_far = np.dot(button_pose_rotation_matrix, local_offset_far)
    world_offset_near = np.dot(button_pose_rotation_matrix, local_offset_near)
    # 初始位置
    pose_far = [-1,-1,-1]
    pose_far[0] = button_pose.pose.position.x + world_offset_far[0]
    pose_far[1] = button_pose.pose.position.y + world_offset_far[1]
    pose_far[2] = button_pose.pose.position.z + world_offset_far[2]
    #left_arm_move_to_pos(pose_far,"Left_arm_Link")

    rospy.sleep(0.2)

    # 按下去
    pose_naer = [-1,-1,-1]
    pose_naer[0] = button_pose.pose.position.x + world_offset_near[0]
    pose_naer[1] = button_pose.pose.position.y + world_offset_near[1]
    pose_naer[2] = button_pose.pose.position.z + world_offset_near[2]
    left_arm_move_to_pos(pose_naer,"Left_arm_Link")

    rospy.sleep(0.2)

    # 初始位置
    pose_far = [-1,-1,-1]
    pose_far[0] = button_pose.pose.position.x + world_offset_far[0]
    pose_far[1] = button_pose.pose.position.y + world_offset_far[1]
    pose_far[2] = button_pose.pose.position.z + world_offset_far[2]
    left_arm_move_to_pos(pose_far,"Left_arm_Link")



def tf_pub(frame_id, child_frame_id, P, R, is_quat=False):
    ''''''
    br = tf2_ros.TransformBroadcaster()
    t = TransformStamped()
    t.header.stamp = rospy.Time.now()
    t.header.frame_id = frame_id
    t.child_frame_id = child_frame_id

    t.transform.translation.x=P[0]
    t.transform.translation.y=P[1]
    t.transform.translation.z=P[2]

    if not is_quat:
        R_object_orientation = np.eye(4)
        R_object_orientation[:3, :3] = R
        quat = quaternion_from_matrix(R_object_orientation)
    else:
        quat = R
    t.transform.rotation.x = quat[0]
    t.transform.rotation.y = quat[1]
    t.transform.rotation.z = quat[2]
    t.transform.rotation.w = quat[3]
    br.sendTransform(t)


def compute_hand_position(P_object, R_object, dimensions, hand_y_toward):
    """
    计算夹爪接触点和姿态
    Parameters:
    - P_object: 物体位置 (3x1 array)
    - R_object: 物体姿态 (3x3 rotation matrix)
    - dimensions: 物体尺寸 (tuple), cylinder: (r, h), cuboid: (l, w, h)
    - hand_y_toward: base_link下 末端tool 的期望 y朝向（灵巧手的虎口）
    Returns:
    - P_grasp: 抓取点的位置
    - grasp_orientation: 抓取姿态 (3x3 rotation matrix)
    """

    r, h = dimensions
    obj_x_dir = R_object[:, 0]
    obj_y_dir = R_object[:, 1]
    obj_z_dir = R_object[:, 2]

    tool_x_dir = -obj_z_dir
    tool_x_dir = tool_x_dir / np.linalg.norm(tool_x_dir)

    if hand_y_toward ==  [0,-1,0]:   #'base_y_nagtive': 对应base的y轴负方向
        tool_y_candidate = np.array([0.0,-1.0,0.0])

        dot_xy = np.abs(np.dot(tool_x_dir,tool_y_candidate))

        if dot_xy > 0.99:
            # 如果tool的x轴方向与base的y轴方向基本平行，就把tool的y轴放到base的z轴重新来一次
            tool_y_candidate = np.array([0.0,0.0,-1.0])
        
        v_perp = tool_y_candidate - np.dot(tool_y_candidate,tool_x_dir) * tool_x_dir
        norm_vp = np.linalg.norm(v_perp)
        
        if norm_vp < 1e-8:
            tool_y_candidate = np.array([0.0,0.0,1.0])
            v_perp = tool_y_candidate - np.dot(tool_y_candidate,tool_x_dir) * tool_x_dir
            norm_vp = np.linalg.norm(v_perp)
    else:    
        # 对应小臂朝向其他方向
        tool_y_candidate = hand_y_toward / np.linalg.norm(hand_y_toward)

        dot_xy = np.abs(np.dot(tool_x_dir,tool_y_candidate))

        if dot_xy > 0.99:
            # 如果tool的x轴方向与期望的y轴方向基本平行 说明杯子倒了。。。。
            return False, None, None
        v_perp = tool_y_candidate - np.dot(tool_y_candidate,tool_x_dir) * tool_x_dir
        norm_vp = np.linalg.norm(v_perp)

    tool_y_dir = v_perp / norm_vp
    tool_z_dir = np.cross(tool_x_dir,tool_y_dir)
    tool_z_dir = tool_z_dir / np.linalg.norm(tool_z_dir)

    grasp_orientation = np.column_stack((tool_x_dir, tool_y_dir, tool_z_dir))
    grasp_poses =[]
    grasp_poses.append({
            "position": P_object + grasp_orientation @ np.array([0.03, r, 0.068])  ,# 沿 y 轴正方向移动r  z轴前靠6cm
            "orientation": grasp_orientation
        })
    # 发布抓取期望位姿 tf 可视化
    tf_pub("base_link", "target_tool_link", grasp_poses[0]["position"], grasp_poses[0]["orientation"])


    return True, grasp_poses[0]["position"],grasp_poses[0]["orientation"]


def compute_new_compensation(current_pos, current_quat, target_pos, target_quat):
    """
    根据当前姿态和目标姿态计算新的补偿四元数。
    
    :param current_quat: [qx, qy, qz, qw]，当前姿态
    :param target_quat: [qx, qy, qz, qw]，目标姿态
    :return: [qx, qy, qz, qw]，新的补偿四元数
    """
    # 计算 delta_quat = target_quat * current_quat^-1
    current_quat_inv = quaternion_inverse(current_quat)
    delta_quat = quaternion_multiply(target_quat, current_quat_inv)

    # 计算 delta_pos = target_pos - current_pos
    delta_pos = target_pos - current_pos
    return delta_pos, delta_quat

def move_to_pose(position , quaternion, link_name, use_oc = False):
    """
    移动手臂到指定位姿。
    :param pose: Pose 消息，目标位姿
    :param link_name: 要移动的链接名称
    """
    global move_group_right_arm
    move_pose_msg = Pose()
    move_pose_msg.position.x = position[0]
    move_pose_msg.position.y = position[1] 
    move_pose_msg.position.z = position[2]
    move_pose_msg.orientation.x = quaternion[0]
    move_pose_msg.orientation.y = quaternion[1]
    move_pose_msg.orientation.z = quaternion[2]
    move_pose_msg.orientation.w = quaternion[3]

    print("adjusted_right_pose",move_pose_msg)
    print(move_group_right_arm.get_current_pose().pose)

    move_group_right_arm.set_pose_target(move_pose_msg)
    plan,traj,planning_time,error_code = move_group_right_arm.plan()
    print(planning_time)

    res_execute = move_group_right_arm.execute(traj, wait=True)
    move_group_right_arm.stop()
    move_group_right_arm.clear_pose_targets()
    rospy.sleep(0.2)

    #if use_oc:
    #    move_group_right_arm.clear_path_constraints()

    if plan and res_execute:
        rospy.loginfo(f"Successfully moved {link_name} to the target pose.")
    else:
        rospy.logwarn(f"Failed to move {link_name} to the target pose.")
    return plan and res_execute

def is_pose_similar(position1, quaternion1, position2, quaternion2, max_translation_error=0.01, max_rotation_error=10.0):
    """
    判断两个时刻的位姿偏差是否足够小。

    参数:
        position1 (array-like): 第一个时刻的位置 [x, y, z]。
        quaternion1 (array-like): 第一个时刻的旋转 [x, y, z, w]。
        position2 (array-like): 第二个时刻的位置 [x, y, z]。
        quaternion2 (array-like): 第二个时刻的旋转 [x, y, z, w]。
        max_translation_error (float): 平移误差阈值 (单位: 米)。
        max_rotation_error (float): 旋转误差阈值 (单位: 度)。

    返回:
        bool: 如果平移误差和旋转误差都在阈值范围内，返回 True；否则返回 False。
    """
    # 计算平移误差
    position1 = np.array(position1)
    position2 = np.array(position2)
    translation_error = np.linalg.norm(position1 - position2)

    # 计算旋转误差
    quat1 = Rotation.from_quat(quaternion1)
    quat2 = Rotation.from_quat(quaternion2)
    relative_rotation = quat1.inv() * quat2
    rotation_error = relative_rotation.magnitude() * (180 / np.pi)  # 转换为度数

    print("translation_error :  " ,translation_error)
    print("rotation_error    :  " ,rotation_error)

    # 判断误差是否足够小
    return translation_error <= max_translation_error and rotation_error <= max_rotation_error


def quat_multiply(quat1, quat2):
    """
    将两个四元数相乘，得到新的四元数。
    格式 qx, qy, qz, qw
    """
    qx1, qy1, qz1, qw1 = quat1
    qx2, qy2, qz2, qw2 = quat2
    
    # 计算四元数乘积
    qx = qw1*qx2 + qx1*qw2 - qy1*qz2 + qz1*qy2
    qy = qw1*qy2 + qy1*qw2 + qx1*qz2 - qz1*qx2
    qz = qw1*qz2 - qx1*qy2 + qy1*qx2 + qz1*qw2
    qw = qw1*qw2 - qx1*qx2 - qy1*qy2 - qz1*qz2
    
    return [qx, qy, qz, qw]

def move_to_stand_by_pose(is_use_compensate=False):
    # 让机械臂回归到待命位姿
    # 在任务开始、抓取完成、放置完成都会执行
    # 如果 需要补偿，则会在归位末端位姿 叠加一个 tool_quat_compensate
    # global tool_quat_compensate
    '''
    pose:       这是ARM_LINK_7的
        position: 
            x: 0.4073865420857997
            y: -0.47497118260638577
            z: 0.9133771081087848
        orientation: 
            x: 0.33585726536946847
            y: -0.5657318676152812
            z: 0.5082700924088744
            w: 0.5557057354623427
    '''
    #stand_by_position = [0.552,-0.271,0.961]   ## 90度 小臂
    #stand_by_quaternion = [0.6524788661440531, -0.2256188689670651, 0.6878808971115478, 0.2240252811412265] #[ 0.7071067811865475 , 0.000 , 0.7071067811865475 , 0.000 ] #虎口朝左
    stand_by_position = [0.705, -0.367, 0.951 + 0.1]
    stand_by_quaternion = [0.551, -0.388, 0.621, 0.400]

    if is_use_compensate:
        stand_by_quaternion = quat_multiply(stand_by_quaternion, tool_quat_compensate)
        #tf_pub("base_link", "tool_quat_compensate", stand_by_position, tool_quat_compensate, is_quat=True)
    
    #tf_pub("base_link", "stand_by_pose", stand_by_position, stand_by_quaternion, is_quat=True)


    res_execute = move_to_pose(stand_by_position, stand_by_quaternion, "Right_Arm_Link7")



def grasp_task(grasp_y_toward, pre_grasp_pose):
    '''

    两次抓取任务重复调用，每次抓取从不同角度、位置靠近
    在桌面上，末端工具坐标系y朝机器人本体左边（-y方向），从上往下套住杯子
    在咖啡机上，y轴朝向咖啡机，从后往前去套住杯子

    '''
    global coffee_task_manager
    # 重复规划机械臂 直到抓取位姿
    falied_times = 0
    grasp_y_toward_copy = copy.deepcopy(grasp_y_toward)
    while not coffee_task_manager.state_flag == 'grasp_hand_arrive':
    
        P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
        quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y, obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]

        # 将四元数转换为旋转矩阵
        R_object = quaternion_matrix(quaternion_object)[:3, :3]

        # 定义物体形状和尺寸（假设是圆柱体）
        shape = 'cylinder'
        dimensions = [0.055, 0.106]  # 直径和高度

        # 计算抓取点位置和抓取姿态
        res_cacu_grasp, P_grasp, grasp_orientation = compute_hand_position(P_object, R_object,  dimensions, hand_y_toward=grasp_y_toward)
        
        # 计算失败 ， 杯子倒了  可以完善解决
        if not res_cacu_grasp:
            rospy.loginfo(f"Faild to caculate grasp Pose !!!!!!!!") #打印时间戳
            falied_times = falied_times + 1
            grasp_y_toward[0] *= 0.5
            if falied_times < 3:
                grasp_y_toward[0] =grasp_y_toward_copy[0] 
                continue
            else:
                # 一直尝试
                continue
                #
                move_to_stand_by_pose()
                coffee_task_manager.state_refresh('stand_by') 
                return False
        # 回复生命
        #falied_times = 0


        # 将抓取姿态扩展为 4x4 齐次变换矩阵
        grasp_orientation_homogeneous = np.eye(4)
        grasp_orientation_homogeneous[:3, :3] = grasp_orientation

        # 将抓取姿态转换为四元数
        grasp_quaternion = quaternion_from_matrix(grasp_orientation_homogeneous)

        if pre_grasp_pose == 'beyond':
            # arm 先到杯子上方 对应tool_link的x轴方向
            local_offset = np.array([0.15, 0.0, 0.0, 1.0])
            #world_offset = np.dot(grasp_orientation, local_offset)
            world_offset = np.dot(quaternion_matrix(grasp_quaternion), local_offset)
            # 如果杯子是反的，需要保证z偏移是正的！
            if world_offset[2] < 0:
                world_offset = - world_offset
        elif pre_grasp_pose == 'front':
            # arm 先到杯子前方 对应tool_link的y轴方向
            local_offset = np.array([0.0, 0.1, 0.0, 1.0])
            world_offset = np.dot(quaternion_matrix(grasp_quaternion), local_offset)

        # 构建预备抓取位姿消息
        P_grasp_offset = [-1.0,-1.0,-1.0]
        P_grasp_offset[0] = P_grasp[0] + world_offset[0]
        P_grasp_offset[1] = P_grasp[1] + world_offset[1]
        P_grasp_offset[2] = P_grasp[2] + world_offset[2]
        
        # 执行
        res_execute = move_to_pose(P_grasp_offset, grasp_quaternion, "Right_Arm_Link7", use_oc = True)

        rospy.loginfo(f"if sucuced to move_to_pose grasp Pose !!!!!!!! {res_execute}") #打印时间戳
        tf_pub("base_link", "pre_grasp", P_grasp_offset, grasp_orientation)


        # 失败3次 就退出归位
        if not res_execute:
            falied_times = falied_times + 1
            if falied_times < 3:
                rospy.loginfo(f"Faild to move_to_pose grasp Pose !!!!!!!! {falied_times}") #打印时间戳
                continue
            else:
                # 一直尝试
                continue
                #
                move_to_stand_by_pose()
                coffee_task_manager.state_refresh('stand_by') 
                return False
        # 回复生命
        falied_times = 0
        ## 到位后验算 看看是否能满足抓取
        new_P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
        new_quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y, obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]

        # 将四元数转换为旋转矩阵
        new_R_object = quaternion_matrix(new_quaternion_object)[:3, :3]

        # 计算抓取点位置和抓取姿态
        res_cacu_grasp, new_P_grasp, new_grasp_orientation = compute_hand_position(new_P_object, new_R_object,  dimensions, hand_y_toward=grasp_y_toward)

        # 将抓取姿态扩展为 4x4 齐次变换矩阵
        new_grasp_orientation_homogeneous = np.eye(4)
        new_grasp_orientation_homogeneous[:3, :3] = new_grasp_orientation

        # 将抓取姿态转换为四元数
        new_grasp_quaternion = quaternion_from_matrix(new_grasp_orientation_homogeneous)

        res_similar = is_pose_similar(P_grasp, grasp_quaternion, new_P_grasp, new_grasp_quaternion, max_translation_error=0.05, max_rotation_error=30.0)     # 容忍5mm 10°
        
        if res_similar:
            coffee_task_manager.state_refresh('grasp_arm_arrive') 
        else:
            continue
        # 机械臂到达 预抓取位姿后 进一步前往抓取位姿

        # 构建预备抓取位姿消息
        res_execute = move_to_pose(new_P_grasp, new_grasp_quaternion , "Right_Arm_Link7")

        # 规划执行失败了 归位 并重置标志位
        if not res_execute:
            # 一直尝试
            continue
            #
            move_to_stand_by_pose()
            coffee_task_manager.state_refresh('stand_by') 
            return False
        
        # 标志位转为 到达抓取位姿
        coffee_task_manager.state_refresh('grasp_hand_arrive')

    # 发送 手抓取
    msg_hand_grasp = Bool()
    msg_hand_grasp.data = False
    hand_control_publisher.publish(msg_hand_grasp)
    rospy.loginfo(f"Publish False at {rospy.get_time()}") #打印时间戳
    rospy.sleep(2)
    
    # 没有反馈 直接置为到达
    coffee_task_manager.state_refresh('grasp_finger_arrive') 


    #########  抓取后 再次计算期望 伪期待抓取位姿 以获得杯子倾斜回正的 末端补偿旋转
    ## 细节计算 抓握后杯子怎么歪的  ag = after_grasp
    ag_P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
    ag_quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y, obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]

    # 将四元数转换为旋转矩阵
    ag_R_object = quaternion_matrix(ag_quaternion_object)[:3, :3]

    # 计算抓取点位置和抓取姿态
    res_cacu_grasp, ag_P_grasp, ag_grasp_orientation = compute_hand_position(ag_P_object, ag_R_object,  dimensions, hand_y_toward=grasp_y_toward)

    # 将抓取姿态扩展为 4x4 齐次变换矩阵
    ag_grasp_orientation_homogeneous = np.eye(4)
    ag_grasp_orientation_homogeneous[:3, :3] = ag_grasp_orientation

    # 将抓取姿态转换为四元数
    ag_grasp_quaternion = quaternion_from_matrix(ag_grasp_orientation_homogeneous)

    # 简单验算一下吧 如果差太多，说明抓取/跟踪 失败
    res_similar = is_pose_similar(new_P_grasp, new_grasp_quaternion, ag_P_grasp, ag_grasp_quaternion, max_translation_error=0.05, max_rotation_error=50.0)     # 容忍15mm 50°
    
    if not res_similar:
        # 发送 手释放
        msg_hand_grasp = Bool()
        msg_hand_grasp.data = True
        hand_control_publisher.publish(msg_hand_grasp)
        rospy.loginfo(f"Publish True at {rospy.get_time()}") #打印时间戳
        rospy.sleep(2)
        move_to_stand_by_pose()
        coffee_task_manager.state_refresh('stand_by') 
        return False
    
    # 计算抓取位姿偏差
    global tool_quat_compensate , tool_pos_compensate
    tool_pos_compensate , tool_quat_compensate = compute_new_compensation(ag_P_grasp , ag_grasp_quaternion, new_P_grasp, new_grasp_quaternion)
    '''
    stand_by_position = [0.705, -0.367, 0.951]
    print("ag_grasp_quaternion",ag_grasp_quaternion)
    print("new_grasp_quaternion",new_grasp_quaternion)
    tf_pub("base_link", "new_grasp_quaternion", stand_by_position, new_grasp_quaternion, is_quat=True)
    tf_pub("base_link", "tool_quat_compensate", stand_by_position, tool_quat_compensate, is_quat=True)
    '''



    # 归一下位置 并刷新一下任务、状态标志位
    move_to_stand_by_pose(is_use_compensate=True)
    coffee_task_manager.state_refresh('grasp_stand_by') # 会将task_flag = "put_on_table" 

    return True



def task_run():
    '''

    开始执行任务了

    '''
    global coffee_task_manager

    # 定义物体形状和尺寸（假设是圆柱体）
    shape = 'cylinder'
    dimensions = [0.055, 0.106]  # 直径和高度

    # 机械臂运动状态
    coffee_task_manager.state_refresh('grasp_arm_go')
    
    res_grasp = grasp_task(grasp_y_toward=[-1,-1,0],pre_grasp_pose='beyond')
    # 3次抓取失败了 直接返回
    if not res_grasp:
        return
    
    falied_times = 0
    ################################################################# 至此 抓取 归位 完成 ###############################################
    while not  coffee_task_manager.state_flag == 'put_finger_arrive':
        ## 开始放置到 咖啡机上
        P_machine_cup = np.array([machine_cup_pose_msg.pose.position.x, machine_cup_pose_msg.pose.position.y, machine_cup_pose_msg.pose.position.z])
        quaternion_machine_cup = [machine_cup_pose_msg.pose.orientation.x, machine_cup_pose_msg.pose.orientation.y, machine_cup_pose_msg.pose.orientation.z, machine_cup_pose_msg.pose.orientation.w]

        ############################################################# 这里手的y轴要 斜45°在期望杯子位姿的xy下，姿态自然，规划更容易成功                        旧 ########冲着自己， 保证能全身（手）而退
        # 将四元数转换为旋转矩阵
        R_machine_cup = quaternion_matrix(quaternion_machine_cup)[:3, :3]

        # 计算放置位置和放置姿态
        # 这里手的y轴要 斜45°在期望杯子位姿的xy下，姿态自然，规划更容易成功
        release_hand_y_toward = [-1, -1 , 0]
        res_cacu_release, P_release, release_orientation = compute_hand_position(P_machine_cup, R_machine_cup,  dimensions, hand_y_toward=release_hand_y_toward)

        # 将抓取姿态扩展为 4x4 齐次变换矩阵
        release_orientation_homogeneous = np.eye(4)
        release_orientation_homogeneous[:3, :3] = release_orientation

        # 将抓取姿态转换为四元数
        release_quaternion = quaternion_from_matrix(release_orientation_homogeneous)

        # 先靠近 然后计算更新一次 旋转补偿 然后再执行
        # arm 先到杯子前方 对应tool_link的y轴方向
        local_offset = np.array([0.0, 0.1, 0.0, 1.0])
        #world_offset = np.dot(release_orientation, local_offset)
        world_offset = np.dot(quaternion_matrix(release_quaternion), local_offset)

        # 构建预备抓取位姿消息
        global tool_quat_compensate,tool_pos_compensate
        P_pre_release = [-1.0, -1.0, -1.0]
        P_pre_release[0] = P_release[0] + world_offset[0] + tool_pos_compensate[0]
        P_pre_release[1] = P_release[1] + world_offset[1] + tool_pos_compensate[1]
        P_pre_release[2] = P_release[2] + world_offset[2] + tool_pos_compensate[2]
        pre_release_quaternion  = quat_multiply(release_quaternion, tool_quat_compensate)   # 先用抓取后的补偿 作为期待

        tf_pub("base_link", "pre_release", P_pre_release, pre_release_quaternion, is_quat=True)
        
        # 执行 接近动作
        res_execute = move_to_pose(P_pre_release, pre_release_quaternion, "Right_Arm_Link7")
        if not res_execute:
            falied_times = falied_times + 1
            if falied_times < 3:
                release_hand_y_toward[0] = 0.33 + release_hand_y_toward[0]
                continue
            else:
                move_to_stand_by_pose()
                coffee_task_manager.state_refresh('stand_by') 
                # 发送 手放置
                msg_hand_release = Bool()
                msg_hand_release.data = True
                hand_control_publisher.publish(msg_hand_release)
                rospy.loginfo(f"Publish True at {rospy.get_time()}") #打印时间戳
                rospy.sleep(2)
                return
            

        ##################################################################### 到达预放置位置 ###########################################################################
        ## 更新 compensate
        rospy.sleep(1)
        ## 细节计算 放置前杯子怎么歪的  br = before_release
        br_P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
        br_quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y, obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]

        # 将四元数转换为旋转矩阵
        br_R_object = quaternion_matrix(br_quaternion_object)[:3, :3]

        # 计算抓取点位置和抓取姿态
        res_cacu_release, br_P_release, br_release_orientation = compute_hand_position(br_P_object, br_R_object,  dimensions, hand_y_toward=release_hand_y_toward)

        # 将抓取姿态扩展为 4x4 齐次变换矩阵
        br_release_orientation_homogeneous = np.eye(4)
        br_release_orientation_homogeneous[:3, :3] = br_release_orientation

        # 将抓取姿态转换为四元数
        br_release_quaternion = quaternion_from_matrix(br_release_orientation_homogeneous)

        # 简单验算一下吧 如果差太多，说明抓取/跟踪 失败
        res_similar = is_pose_similar(P_pre_release, pre_release_quaternion, br_P_release, br_release_quaternion, max_translation_error=0.08, max_rotation_error=50.0)     # 容忍2cm 50°
        
        if not res_similar:
            # 发送 手释放
            msg_hand_grasp = Bool()
            msg_hand_grasp.data = True
            hand_control_publisher.publish(msg_hand_grasp)
            rospy.loginfo(f"Publish True at {rospy.get_time()}") #打印时间戳
            rospy.sleep(2)
            move_to_stand_by_pose()
            coffee_task_manager.state_refresh('stand_by') 
            return False
        
        # 计算抓取位姿偏差
        #global tool_quat_compensate
        P_release_tmp = [-1.0, -1.0, -1.0]
        P_release_tmp[0] = P_release[0] + world_offset[0]
        P_release_tmp[1] = P_release[1] + world_offset[1]
        P_release_tmp[2] = P_release[2] + world_offset[2]
        tool_pos_compensate_inc, tool_quat_compensate_inc = compute_new_compensation(br_P_object, br_release_quaternion,P_release_tmp, release_quaternion)

        tool_quat_compensate = quat_multiply(tool_quat_compensate, tool_quat_compensate_inc)
        tool_pos_compensate = tool_pos_compensate + tool_pos_compensate_inc
   

        release_quaternion_compensate  = quat_multiply(release_quaternion, tool_quat_compensate)   # 用抓取后的补偿
        P_release_compensate = P_release + tool_pos_compensate

        tf_pub("base_link", "P_release_compensate", P_release_compensate, release_quaternion_compensate, is_quat=True)

        # 执行
        # 手动 目测补偿
        P_release[0] = P_release[0] + 0.023
        P_release[1] = P_release[1] - 0.01
        res_execute = move_to_pose(P_release, release_quaternion_compensate, "Right_Arm_Link7")

        # 失败3次 就退出归位
        if not res_execute:
            falied_times = falied_times + 1
            if falied_times < 3:
                continue
            else:
                move_to_stand_by_pose()
                coffee_task_manager.state_refresh('stand_by') 
                # 发送 手放置
                msg_hand_release = Bool()
                msg_hand_release.data = True
                hand_control_publisher.publish(msg_hand_release)
                rospy.loginfo(f"Publish True at {rospy.get_time()}") #打印时间戳
                rospy.sleep(2)
                return

        # 回复生命
        falied_times = 0
        coffee_task_manager.state_refresh('put_arm_arrive') 

        # 发送 手放置
        #msg_hand_release = Bool()
        #msg_hand_release.data = False
        #hand_control_publisher.publish(msg_hand_release)
        #rospy.loginfo(f"Publish False at {rospy.get_time()}") #打印时间戳
        #rospy.sleep(2)
        
        # 没有反馈 直接置为到达
        coffee_task_manager.state_refresh('put_finger_arrive')  ## 刷新咖啡状态 到1

    ############################################################## 至此 放置 归位 完成 ###########################################################

    # 开始抓杯子（有咖啡）
    # 通过咖啡机平面法向量 得到抓取 的 y轴朝向
    #res_grasp = grasp_task([0,-1,0],'front')  ###################################### 还没改过来的
    # 3次抓取失败了 直接返回
    #if not res_grasp:
    #    return


    ############################################################### v2版本省略 放table 和再抓取了 ##############################################


    ################################################################# 开始按咖啡机

    press_button_callback()

    #rospy.sleep(50)
    rospy.sleep(5)

    # 递给人的速度提高
    global move_group_right_arm
    move_group_right_arm.set_max_velocity_scaling_factor(0.3)
    move_group_right_arm.set_max_acceleration_scaling_factor(0.3)
    ## 递给人之前 先撤出咖啡机
    res_execute = move_to_pose(P_pre_release, pre_release_quaternion, "Right_Arm_Link7")
    ## 并回到stand by 位置
    move_to_stand_by_pose(is_use_compensate=True)
    coffee_task_manager.state_refresh('grasp_stand_by') # 会将task_flag = "handover" 

    ################################################################# 至此 抓取 归位 完成 ###############################################

    ## 这里需要找到人手才能开始 规划放置姿态
    # 加一个循环等待handover有效到来
    ## 开始放置任务 handover
    while not  coffee_task_manager.state_flag == 'put_finger_arrive':
        P_handover = np.array([handover_pose_msg.pose.position.x, handover_pose_msg.pose.position.y, handover_pose_msg.pose.position.z])
        handover_quaternion = [handover_pose_msg.pose.orientation.x, handover_pose_msg.pose.orientation.y, handover_pose_msg.pose.orientation.z, handover_pose_msg.pose.orientation.w]

        ############################################################# 这里手的y轴要冲着自己， 保证能全身（手）而退

        # 倾斜角补偿
        handover_quaternion_compensate  = quat_multiply(handover_quaternion, tool_quat_compensate)   # 用抓取后的补偿
        P_handover_compensate = P_handover + tool_pos_compensate
        # 执行
        res_execute = move_to_pose(P_handover_compensate, handover_quaternion_compensate, "Right_Arm_Link7")

        # 失败3次 就退出归位
        if not res_execute:
            falied_times = falied_times + 1
            if falied_times < 3:
                continue
            else:
                continue
                move_to_stand_by_pose()
                coffee_task_manager.state_refresh('stand_by') 
                # 发送 手放置
                msg_hand_release = Bool()
                msg_hand_release.data = True
                hand_control_publisher.publish(msg_hand_release)
                rospy.loginfo(f"Publish True at {rospy.get_time()}") #打印时间戳
                rospy.sleep(2)
                return

        # 回复生命
        falied_times = 0
        coffee_task_manager.state_refresh('put_arm_arrive') 

        # 发送 手放置
        msg_hand_release = Bool()
        msg_hand_release.data = True
        hand_control_publisher.publish(msg_hand_release)
        rospy.loginfo(f"Publish True at {rospy.get_time()}") #打印时间戳
        rospy.sleep(2)
        
        # 没有反馈 直接置为到达
        coffee_task_manager.state_refresh('put_finger_arrive') 

    move_to_stand_by_pose()
    coffee_task_manager.state_refresh('stand_by') 


if __name__ == '__main__':
    # 初始化 ROS 节点
    rospy.init_node('coffee_task_manager_node', anonymous=True)

    # MoveIt 初始化
    move_group_right_arm = MoveGroupCommander("right_arm")
    move_group_right_arm.set_end_effector_link("right_tool_link")
    move_group_right_arm.set_planning_time(0.5) # 0.1s 的规划用时限制
    move_group_right_arm.set_max_velocity_scaling_factor(0.2)
    move_group_right_arm.set_max_acceleration_scaling_factor(0.2)
    print(move_group_right_arm.get_planning_frame())

    # 左臂 初始化
    move_group_left_arm = MoveGroupCommander("left_arm")
    move_group_left_arm.set_end_effector_link("Left_Index_Finger_Link3")
    move_group_left_arm.set_planning_time(0.5) # 0.1s 的规划用时限制
    move_group_left_arm.set_max_velocity_scaling_factor(0.3)
    move_group_left_arm.set_max_acceleration_scaling_factor(0.3)

    hand_control_publisher = rospy.Publisher("/hand_control_topic", Bool, queue_size=1)


    rospy.Subscriber("/obj_pose", PoseStamped, obj_pose_callback, queue_size=1)

    rospy.Subscriber("/button_pose", PoseStamped, button_pose_callback, queue_size=1)

    rospy.Subscriber("/machine_cup_pose", PoseStamped, machine_cup_pose_callback, queue_size=1)

    rospy.Subscriber("/handover_pose", PoseStamped, handover_pose_callback, queue_size=1)

    rospy.Subscriber("/upper_body_command", String, upper_body_callback, queue_size=1)


    coffee_task_manager = Coffee_Task_Manager()

    rospy.spin()

